#ifndef MahonyAHRS_h
#define MahonyAHRS_h
#include <math.h>

//--------------------------------------------------------------------------------------------
// Variable declaration

class Mahony
{
private:
  float twoKp;                                 // 2 * proportional gain (Kp)
  float twoKi;                                 // 2 * integral gain (Ki)
  float q0, q1, q2, q3;                        // quaternion of sensor frame relative to auxiliary frame
  float integralFBx, integralFBy, integralFBz; // integral error terms scaled by Ki
  float invSampleFreq;
  float roll, pitch, yaw;
  char anglesComputed;
  static float invSqrt(float x);
  void computeAngles();

  //-------------------------------------------------------------------------------------------
  // Function declarations

public:
  Mahony();
  void begin(float sampleFrequency) { invSampleFreq = 1.0f / sampleFrequency; }
  void update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
  void updateIMU(float gx, float gy, float gz, float ax, float ay, float az);
  void getQuaternion(float *q)
  {
    q[0] = q0;
    q[1] = q1;
    q[2] = q2;
    q[3] = q3;
  }
  float getRoll()
  {
    if (!anglesComputed)
      computeAngles();
    return roll * 57.29578f;
  }
  float getPitch()
  {
    if (!anglesComputed)
      computeAngles();
    return pitch * 57.29578f;
  }
  float getYaw()
  {
    if (!anglesComputed)
      computeAngles();
    return yaw * 57.29578f + 180.0f;
  }
  float getRollRadians()
  {
    if (!anglesComputed)
      computeAngles();
    return roll;
  }
  float getPitchRadians()
  {
    if (!anglesComputed)
      computeAngles();
    return pitch;
  }
  float getYawRadians()
  {
    if (!anglesComputed)
      computeAngles();
    return yaw;
  }
};

#endif
